/**
 * @file mpu6050_demo.c
 * @author Jaychen (719095404@qq.com)
 * @brief MPU6050 DMP演示例程
 * @version 0.1
 * @date 2023-11-04
 *
 * @copyright Copyright (c) 2023
 *
 */
#include "stm32f10x.h"
#include "delay.h"
#include "mpu6050_i2c.h"
#include "usart.h"

#include "mltypes.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "log.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "eMPL_outputs.h"
#include "mpu.h"
#include "inv_mpu.h"
#include "log.h"
#include "packet.h"
#include "mpu6050_demo.h"
#include <stdio.h>

/* Private typedef -----------------------------------------------------------*/
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
/* Starting sampling rate. */
#define DEFAULT_MPU_HZ (20)
#define TEMP_READ_MS (500)

unsigned char *mpl_key = (unsigned char *)"eMPL 5.1";
volatile mpu6050_msg_t mpu6050_data = {0};
struct rx_s
{
    unsigned char header[3];
    unsigned char cmd;
};

struct hal_s
{
    unsigned char lp_accel_mode;
    unsigned char sensors;
    unsigned char dmp_on;
    unsigned char wait_for_tap;
    volatile unsigned char new_gyro; // 外部中断标志，以通知MPL 库有新的数据
    unsigned char motion_int_mode;
    unsigned long no_dmp_hz;
    unsigned long next_pedo_ms;
    unsigned long next_temp_ms;
    unsigned long next_compass_ms;
    unsigned int report;
    unsigned short dmp_features;
    struct rx_s rx;
};
static struct hal_s hal = {0};

/* Platform-specific information. Kinda like a boardfile. */
struct platform_data_s
{
    signed char orientation[9];
};

/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from the
 * driver(s).
 * TODO: The following matrices refer to the configuration on internal test
 * boards at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static struct platform_data_s gyro_pdata = {

    .orientation = {1, 0, 0,
                    0, 1, 0,
                    0, 0, 1}};

/* Private define ------------------------------------------------------------*/

/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* ---------------------------------------------------------------------------*/
#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))

/*函数功能：
 */

/**
 * @brief 根据匿名最新上位机协议写的显示姿态的程序（上位机V7.16版本）
 *具体协议说明请查看上位机软件的帮助说明。
 *
 * @param Pitch 俯仰角
 * @param Roll 翻滚角
 * @param Yaw 偏航角
 */
void Data_Send_Status(float Pitch, float Roll, float Yaw)
{
    unsigned char i = 0;
    unsigned char _cnt = 0, sum = 0, addcheck = 0;
    unsigned int _temp;

    u8 data_to_send[50];

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xFF;
    data_to_send[_cnt++] = 0x03; // 功能码
    data_to_send[_cnt++] = 7;    // 数据长度

    _temp = (int)(Roll * 100);
    data_to_send[_cnt++] = BYTE0(_temp);
    data_to_send[_cnt++] = BYTE1(_temp);

    _temp = 0 - (int)(Pitch * 100);
    data_to_send[_cnt++] = BYTE0(_temp);
    data_to_send[_cnt++] = BYTE1(_temp);

    _temp = (int)(Yaw * 100);
    data_to_send[_cnt++] = BYTE0(_temp);
    data_to_send[_cnt++] = BYTE1(_temp);

    _temp = 0; // 融合状态未设置
    data_to_send[_cnt++] = BYTE0(_temp);
    for (i = 0; i < _cnt; i++)
    {
        sum += data_to_send[i]; // 和校验
        addcheck += sum;        // 附加校验
    }

    data_to_send[_cnt++] = sum;
    data_to_send[_cnt++] = addcheck;

    // 串口发送数据
    for (i = 0; i < _cnt; i++)
        Uart_SendByte(data_to_send[i]);
}

/*函数功能：根据匿名最新上位机协议写的显示传感器数据（上位机V7.16版本）
 *具体协议说明请查看上位机软件的帮助说明。
 */
void Send_Data(int16_t *Gyro, int16_t *Accel)
{
    unsigned char i = 0;
    unsigned char _cnt = 0, sum = 0, addcheck = 0;
    u8 data_to_send[50];

    data_to_send[_cnt++] = 0xAA;
    data_to_send[_cnt++] = 0xFF;
    data_to_send[_cnt++] = 0x01;
    data_to_send[_cnt++] = 13;

    data_to_send[_cnt++] = BYTE0(Accel[0]);
    data_to_send[_cnt++] = BYTE1(Accel[0]); // 加速度x

    data_to_send[_cnt++] = BYTE0(Accel[1]);
    data_to_send[_cnt++] = BYTE1(Accel[1]); // 加速度y

    data_to_send[_cnt++] = BYTE0(Accel[2]);
    data_to_send[_cnt++] = BYTE1(Accel[2]); // 加速度z

    data_to_send[_cnt++] = BYTE0(Gyro[0]);
    data_to_send[_cnt++] = BYTE1(Gyro[0]); // 陀螺仪x

    data_to_send[_cnt++] = BYTE0(Gyro[1]);
    data_to_send[_cnt++] = BYTE1(Gyro[1]); // 陀螺仪y

    data_to_send[_cnt++] = BYTE0(Gyro[2]);
    data_to_send[_cnt++] = BYTE1(Gyro[2]); // 陀螺仪z
    data_to_send[_cnt++] = 0;              // 震动状态未设置

    for (i = 0; i < _cnt; i++)
    {
        sum += data_to_send[i]; // 和校验
        addcheck += sum;        // 附加校验
    }

    data_to_send[_cnt++] = sum;
    data_to_send[_cnt++] = addcheck;

    // 串口发送数据
    for (i = 0; i < _cnt; i++)
        Uart_SendByte(data_to_send[i]);
}

extern struct inv_sensor_cal_t sensors;

/* Get data from MPL.
 * TODO: Add return values to the inv_get_sensor_type_xxx APIs to differentiate
 * between new and stale data.
 */
/* 从MOL 中获取数据 MPL.
 */
static void read_from_mpl(void)
{
    long data[9];
    int8_t accuracy;

    unsigned long timestamp;

    MPU_DEBUG_FUNC();

    /*********发送数据到匿名四轴上位机**********/
    /*获取欧拉角*/
    if (inv_get_sensor_type_euler(data, &accuracy, (inv_time_t *)&timestamp))
    {
        float Pitch, Roll, Yaw;
        // inv_get_sensor_type_euler 读出的数据是Q16 格式，所以左移16 位.
        Pitch = data[0] * 1.0 / (1 << 16); // 欧拉角x轴旋转的值,和俯仰角的值一样大小
        Roll = data[1] * 1.0 / (1 << 16);  // 欧拉角y轴旋转的值,和翻滚角的值一样相反
        Yaw = data[2] * 1.0 / (1 << 16);   // 欧拉角z轴旋转的值,和偏航角的值一样相反

        // /*向匿名上位机发送姿态*/
        Data_Send_Status(Pitch, Roll, Yaw);
        // mdelay(10);
        // // 俯仰角 -翻滚角 -偏航角
        // // printf("===============Pitch=%0.2f, Roll=%0.2f, Yaw=%0.2f================\n", Pitch, Roll, Yaw);
        
				// /*向匿名上位机发送原始数据*/
        //Send_Data((int16_t *)&sensors.gyro.raw, (int16_t *)&sensors.accel.raw);
			
        mpu6050_data.Pitch = Pitch;
        mpu6050_data.Roll = Roll;
        mpu6050_data.Yaw = Yaw;
        mpu6050_data.gyro_x = sensors.gyro.raw[0];
        mpu6050_data.gyro_y = sensors.gyro.raw[1];
        mpu6050_data.gyro_z = sensors.gyro.raw[2];
        mpu6050_data.accel_x = sensors.accel.raw[0];
        mpu6050_data.accel_y = sensors.accel.raw[1];
        mpu6050_data.accel_z = sensors.accel.raw[2];
        mpu6050_data.flag = 1;
    }
}

// 自检
static inline void run_self_test(void)
{
    int result;
    long gyro[3], accel[3];
    MPU_DEBUG_FUNC();
#if defined(MPU6500) || defined(MPU9250)
    result = mpu_run_6500_self_test(gyro, accel, 0);
#elif defined(MPU6050) || defined(MPU9150)
    result = mpu_run_self_test(gyro, accel);
#endif
    if (result == 0x7)
    {
        MPL_LOGI("Passed!\n");
        MPL_LOGI("accel: %7.4f %7.4f %7.4f\n",
                 accel[0] / 65536.f,
                 accel[1] / 65536.f,
                 accel[2] / 65536.f);
        MPL_LOGI("gyro: %7.4f %7.4f %7.4f\n",
                 gyro[0] / 65536.f,
                 gyro[1] / 65536.f,
                 gyro[2] / 65536.f);
        /* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/

        /* Push the calibrated data to the MPL library.
         *
         * MPL expects biases in hardware units << 16, but self test returns
         * biases in g's << 16.
         */
        unsigned short accel_sens;
        float gyro_sens;

        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        inv_set_accel_bias(accel, 3);
        mpu_get_gyro_sens(&gyro_sens);
        gyro[0] = (long)(gyro[0] * gyro_sens);
        gyro[1] = (long)(gyro[1] * gyro_sens);
        gyro[2] = (long)(gyro[2] * gyro_sens);
        inv_set_gyro_bias(gyro, 3);
    }
    else
    {
        if (!(result & 0x1))
            MPL_LOGE("Gyro failed.\n");
        if (!(result & 0x2))
            MPL_LOGE("Accel failed.\n");
        if (!(result & 0x4))
            MPL_LOGE("Compass failed.\n");
    }
}

/**
 * @brief mpu6050初始化函数
 *
 */
void mpu6050_init(void)
{
    inv_error_t result;
    unsigned char accel_fsr = 0;
    unsigned short gyro_rate, gyro_fsr;

    struct int_param_s int_param;

    // SysTick_Init();

    // /* 串口通信初始化 */
    // uart_init();

    // // MPU6050中断引脚
    // EXTI_Pxy_Config();
    // I2C初始化
    I2C_Bus_Init();

    printf("mpu 6050 test start");
    
    result = mpu_init(&int_param); // 配置采样率，精度等
    if (result)
    {
        MPL_LOGE("Could not initialize gyro.result =  %d\n", result); // 初始化失败
    }
    else
    {
        // MPU6050初始化成功
    }

    result = inv_init_mpl(); // 函数初始化MPL软件库
    if (result)
    {
        MPL_LOGE("Could not initialize MPL.\n");
    }

    /* 计算6 轴和9 轴传感器的四元数*/
    inv_enable_quaternion();
    inv_enable_9x_sensor_fusion();

    /* Update gyro biases when not in motion.
     * WARNING: These algorithms are mutually exclusive.
     *无运动状态时更新陀螺仪
     */
    inv_enable_fast_nomot();

    /*当温度变化时更新陀螺仪*/
    inv_enable_gyro_tc();

    /* 允许read_from_mpl 使用MPLAPIs. */
    inv_enable_eMPL_outputs();

    result = inv_start_mpl();
    if (result == INV_ERROR_NOT_AUTHORIZED)
    {
        while (1)
        {
            MPL_LOGE("Not authorized.\n");
        }
    }
    if (result)
    {
        MPL_LOGE("Could not start the MPL.\n");
    }
    /* 设置寄存器，开启陀螺仪、加速度传感器 */
    /* 唤醒所有传感器 */
    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);

    /* 把陀螺仪及加速度数据放进FIFO */
    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    /**设置采样率*/
    mpu_set_sample_rate(DEFAULT_MPU_HZ);

    /* 重新读取配置，确认前面的设置成功 */
    mpu_get_sample_rate(&gyro_rate);
    mpu_get_gyro_fsr(&gyro_fsr);
    mpu_get_accel_fsr(&accel_fsr);

    /*使用MPL 同步配置 */
    /* 设置每毫秒的采样率*/
    inv_set_gyro_sample_rate(1000000L / gyro_rate);
    inv_set_accel_sample_rate(1000000L / gyro_rate);

    /* 设置chip-to-body 原点矩阵.* 设置硬件单位为dps/g's/degrees 因子.*/
    inv_set_gyro_orientation_and_scale(
        inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
        (long)gyro_fsr << 15);
    inv_set_accel_orientation_and_scale(
        inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
        (long)accel_fsr << 15);

    /* Initialize HAL state variables. */
    /* 初始化硬件状态相关的变量. */
    hal.sensors = ACCEL_ON | GYRO_ON;

    hal.dmp_on = 0;
    hal.report = 0;
    hal.rx.cmd = 0;
    hal.next_pedo_ms = 0;
    hal.next_compass_ms = 0;
    hal.next_temp_ms = 0;

    /* 初始化DMP 步骤:
    * 1. 调用 dmp_load_motion_driver_firmware().
    它会把inv_mpu_dmp_motion_driver.h 文件中的 DMP 固件写入到MPU 的存储空间
    * 2. 把陀螺仪和加速度的原始数据矩阵送入DMP.
    * 3. 注册姿态回调函数. 除非相应的特性使能了，否则该回调函数不会被执行
    * 4. 调用 dmp_enable_feature(mask) 使能不同的特性.
    * 5. 调用 dmp_set_fifo_rate(freq) 设置DMP 输出频率.
    * 6. 调用特定的特性控制相关的函数.
    *
    * 调用 mpu_set_dmp_state(1)使能DMP. 该函数可在DMP 运行时被重复调用设置使能或关闭
    *
    * 以下是 inv_mpu_dmp_motion_driver.c 文件中的DMP 固件提供的特性的简介：
    * DMP_FEATURE_LP_QUAT: 使用DMP 以200Hz 的频率产生一个只包含陀螺仪的四元数数据
    以高速的状态解算陀螺仪数据，减少错误(相对于使用MCU 以一个低采样率的方式采样计算)
    * DMP_FEATURE_6X_LP_QUAT: 使用DMP 以200Hz 的频率产生 陀螺仪/加速度 四元数 .
    它不能与前面的DMP_FEATURE_LP_QUAT 同时使用
    * DMP_FEATURE_TAP: 检测 X, Y, 和 Z 轴.
    *DMP_FEATURE_ANDROID_ORIENT: 谷歌屏幕翻转算法. 当屏幕翻转时，在四个方向产生一个事件
    * DMP_FEATURE_GYRO_CAL: 若8s 内都没有运动，计算陀螺仪的数据
    * DMP_FEATURE_SEND_RAW_ACCEL: 添加原始 加速度 数据到FIFO.
    * DMP_FEATURE_SEND_RAW_GYRO: 添加原始 陀螺仪 数据到FIFO.
    * DMP_FEATURE_SEND_CAL_GYRO: 添加 校准后的 陀螺仪 数据到FIFO.
    它不能与 DMP_FEATURE_SEND_RAW_GYRO.同时使用
    */
    dmp_load_motion_driver_firmware();
    dmp_set_orientation(
        inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
    hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                       DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
                       DMP_FEATURE_GYRO_CAL;
    dmp_enable_feature(hal.dmp_features);
    dmp_set_fifo_rate(DEFAULT_MPU_HZ);
    mpu_set_dmp_state(1); // 使能DMP
    hal.dmp_on = 1;       // 标记使能DMP
    run_self_test();      // 自检
}

/**更新获取到的数据接口*/
void mpu6050_data_updata(void)
{
    unsigned char new_temp = 0;
    unsigned long timestamp;

    unsigned long sensor_timestamp;
    int new_data = 0;
    get_tick(&timestamp);
    /* 温度数据不需要与陀螺仪数据那每次都采样，这里设置隔一段时间采样 */
    if (timestamp > hal.next_temp_ms)
    {
        hal.next_temp_ms = timestamp + TEMP_READ_MS;
        new_temp = 1;
    }
    if (hal.dmp_on) // DMP打开
    {
        short gyro[3], accel_short[3], sensors;
        unsigned char more;
        long accel[3], quat[4], temperature;
        /* 当使用DMP 时，本函数从FIFO 读取新的数据
         * FIFO 中存储了陀螺仪、加速度、四元数及手势数据.
         * 传感器参数可告知调用者哪种有新数据
         * 例如, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), 那么FIFO 中就不包含加速度数据
         * 手势数据的解算由是否产生了手势运动事件来触发
         * 若产生了事件，应用函数会使用回调函数来通知
         * 如果fifo有剩余的数据，more不为空
         */
        dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);

        if (sensors & INV_XYZ_GYRO) // 陀螺仪数据
        {
            /* Push the new data to the MPL. */
            /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
            inv_build_gyro(gyro, sensor_timestamp);
            new_data = 1;
            if (new_temp) // 获取温度时间到
            {
                new_temp = 0;
                /* Temperature only used for gyro temp comp. */
                /* 温度数据只用于陀螺仪的暂时计算 */
                mpu_get_temperature(&temperature, &sensor_timestamp);
                inv_build_temp(temperature, sensor_timestamp);
                // temperature = temperature * 1.0 / (1 << 16); // Q16格式转正常10进制
                // printf("temperature=%d\n", temperature);
            }
        }
        if (sensors & INV_XYZ_ACCEL) // 加速度数据
        {
            accel[0] = (long)accel_short[0];
            accel[1] = (long)accel_short[1];
            accel[2] = (long)accel_short[2];
            /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
            inv_build_accel(accel, 0, sensor_timestamp);
            new_data = 1;
        }
        if (sensors & INV_WXYZ_QUAT) // 四元数数据
        {
            /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
            inv_build_quat(quat, 0, sensor_timestamp);
            new_data = 1;
        }
    }
    if (new_data)
    {
        /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
        inv_execute_on_data(); // 有新数据，更新内部所有数据及状态
        /* 本函数读取补偿的后的传感器数据和经过MPL 传感器融合后输出的数据
            输出的格式见 in eMPL_outputs.c 文件.
            这个函数在主机需要数据的时候调用即可，对调用频率无要求。
        */
        read_from_mpl();
    }
}

///**
// * @brief mpu6050示例程序
// *
// * @return int
// */
//int mpu6050_demo(void)
//{

//    inv_error_t result;
//    unsigned char accel_fsr, new_temp = 0;
//    unsigned short gyro_rate, gyro_fsr;
//    unsigned long timestamp;
//    struct int_param_s int_param;

//    SysTick_Init();

//    /* 串口通信初始化 */
//    USART_Config();  

//    // // MPU6050中断引脚
//    // EXTI_Pxy_Config();
//    // I2C初始化
//    I2C_Bus_Init();

//    printf("mpu 6050 test start");

//    result = mpu_init(&int_param); // 配置采样率，精度等
//    if (result)
//    {
//        MPL_LOGE("Could not initialize gyro.result =  %d\n", result); // 初始化失败
//    }
//    else
//    {
//        // MPU6050初始化成功
//    }

//    /* If you're not using an MPU9150 AND you're not using DMP features, this
//     * function will place all slaves on the primary bus.
//     */
//    // mpu_set_bypass(1);

//    result = inv_init_mpl(); // 函数初始化MPL软件库
//    if (result)
//    {
//        MPL_LOGE("Could not initialize MPL.\n");
//    }

//    /* Compute 6-axis and 9-axis quaternions. */
//    /* 计算6 轴和9 轴传感器的四元数*/
//    inv_enable_quaternion();
//    inv_enable_9x_sensor_fusion();
//    /* The MPL expects compass data at a constant rate (matching the rate
//     * passed to inv_set_compass_sample_rate). If this is an issue for your
//     * application, call this function, and the MPL will depend on the
//     * timestamps passed to inv_build_compass instead.
//     *
//     * inv_9x_fusion_use_timestamps(1);
//     */

//    /* This function has been deprecated.
//     * inv_enable_no_gyro_fusion();
//     */

//    /* Update gyro biases when not in motion.
//     * WARNING: These algorithms are mutually exclusive.
//     *无运动状态时更新陀螺仪
//     */
//    inv_enable_fast_nomot();
//    /* inv_enable_motion_no_motion(); */
//    /* inv_set_no_motion_time(1000); */

//    /* Update gyro biases when temperature changes. */
//    /*当温度变化时更新陀螺仪*/
//    inv_enable_gyro_tc();

//    /* If you need to estimate your heading before the compass is calibrated,
//     * enable this algorithm. It becomes useless after a good figure-eight is
//     * detected, so we'll just leave it out to save memory.
//     * inv_enable_heading_from_gyro();
//     */

//    /* Allows use of the MPL APIs in read_from_mpl. */
//    /* 允许read_from_mpl 使用MPLAPIs. */
//    inv_enable_eMPL_outputs();

//    result = inv_start_mpl();
//    if (result == INV_ERROR_NOT_AUTHORIZED)
//    {
//        while (1)
//        {
//            MPL_LOGE("Not authorized.\n");
//        }
//    }
//    if (result)
//    {
//        MPL_LOGE("Could not start the MPL.\n");
//    }

//    /* 设置寄存器，开启陀螺仪、加速度传感器 */
//    /* 唤醒所有传感器 */
//    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);

//    /* Push both gyro and accel data into the FIFO. */
//    /* 把陀螺仪及加速度数据放进FIFO */
//    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//    /**设置采样率*/
//    mpu_set_sample_rate(DEFAULT_MPU_HZ);

//    /* Read back configuration in case it was set improperly. */
//    /* 重新读取配置，确认前面的设置成功 */
//    mpu_get_sample_rate(&gyro_rate);
//    mpu_get_gyro_fsr(&gyro_fsr);
//    mpu_get_accel_fsr(&accel_fsr);

//    /* Sync driver configuration with MPL. */
//    /* Sample rate expected in microseconds. */
//    /*使用MPL 同步配置 */
//    /* 设置每毫秒的采样率*/
//    inv_set_gyro_sample_rate(1000000L / gyro_rate);
//    inv_set_accel_sample_rate(1000000L / gyro_rate);

//    /* Set chip-to-body orientation matrix.
//     * Set hardware units to dps/g's/degrees scaling factor.
//     */
//    /* 设置chip-to-body 原点矩阵.* 设置硬件单位为dps/g's/degrees 因子.*/
//    inv_set_gyro_orientation_and_scale(
//        inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
//        (long)gyro_fsr << 15);
//    inv_set_accel_orientation_and_scale(
//        inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
//        (long)accel_fsr << 15);

//    /* Initialize HAL state variables. */
//    /* 初始化硬件状态相关的变量. */
//    hal.sensors = ACCEL_ON | GYRO_ON;

//    hal.dmp_on = 0;
//    hal.report = 0;
//    hal.rx.cmd = 0;
//    hal.next_pedo_ms = 0;
//    hal.next_compass_ms = 0;
//    hal.next_temp_ms = 0;

//    /* Compass reads are handled by scheduler. */
//    /* 获取时间戳（ms) */
//    get_tick(&timestamp);

//    /* To initialize the DMP:
//     * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
//     *    inv_mpu_dmp_motion_driver.h into the MPU memory.
//     * 2. Push the gyro and accel orientation matrix to the DMP.
//     * 3. Register gesture callbacks. Don't worry, these callbacks won't be
//     *    executed unless the corresponding feature is enabled.
//     * 4. Call dmp_enable_feature(mask) to enable different features.
//     * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.
//     * 6. Call any feature-specific control functions.
//     *
//     * To enable the DMP, just call mpu_set_dmp_state(1). This function can
//     * be called repeatedly to enable and disable the DMP at runtime.
//     *
//     * The following is a short summary of the features supported in the DMP
//     * image provided in inv_mpu_dmp_motion_driver.c:
//     * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at
//     * 200Hz. Integrating the gyro data at higher rates reduces numerical
//     * errors (compared to integration on the MCU at a lower sampling rate).
//     * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at
//     * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.
//     * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.
//     * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers
//     * an event at the four orientations where the screen should rotate.
//     * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of
//     * no motion.
//     * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.
//     * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.
//     * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
//     * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
//     */
//    /* 初始化DMP 步骤:
//    * 1. 调用 dmp_load_motion_driver_firmware().
//    它会把inv_mpu_dmp_motion_driver.h 文件中的 DMP 固件写入到MPU 的存储空间
//    * 2. 把陀螺仪和加速度的原始数据矩阵送入DMP.
//    * 3. 注册姿态回调函数. 除非相应的特性使能了，否则该回调函数不会被执行
//    * 4. 调用 dmp_enable_feature(mask) 使能不同的特性.
//    * 5. 调用 dmp_set_fifo_rate(freq) 设置DMP 输出频率.
//    * 6. 调用特定的特性控制相关的函数.
//    *
//    * 调用 mpu_set_dmp_state(1)使能DMP. 该函数可在DMP 运行时被重复调用设置使能或关闭
//    *
//    * 以下是 inv_mpu_dmp_motion_driver.c 文件中的DMP 固件提供的特性的简介：
//    * DMP_FEATURE_LP_QUAT: 使用DMP 以200Hz 的频率产生一个只包含陀螺仪的四元数数据
//    以高速的状态解算陀螺仪数据，减少错误(相对于使用MCU 以一个低采样率的方式采样计算)
//    * DMP_FEATURE_6X_LP_QUAT: 使用DMP 以200Hz 的频率产生 陀螺仪/加速度 四元数 .
//    它不能与前面的DMP_FEATURE_LP_QUAT 同时使用
//    * DMP_FEATURE_TAP: 检测 X, Y, 和 Z 轴.
//    *DMP_FEATURE_ANDROID_ORIENT: 谷歌屏幕翻转算法. 当屏幕翻转时，在四个方向产生一个事件
//    * DMP_FEATURE_GYRO_CAL: 若8s 内都没有运动，计算陀螺仪的数据
//    * DMP_FEATURE_SEND_RAW_ACCEL: 添加原始 加速度 数据到FIFO.
//    * DMP_FEATURE_SEND_RAW_GYRO: 添加原始 陀螺仪 数据到FIFO.
//    * DMP_FEATURE_SEND_CAL_GYRO: 添加 校准后的 陀螺仪 数据到FIFO.
//    它不能与 DMP_FEATURE_SEND_RAW_GYRO.同时使用
//    */

//    dmp_load_motion_driver_firmware();
//    dmp_set_orientation(
//        inv_orientation_matrix_to_scalar(gyro_pdata.orientation));

//    /*
//     * Known Bug -
//     * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate
//     * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once
//     * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz
//     * there will be a 25Hz interrupt from the MPU device.
//     *
//     * There is a known issue in which if you do not enable DMP_FEATURE_TAP
//     * then the interrupts will be at 200Hz even if fifo rate
//     * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP
//     *
//     * DMP sensor fusion works only with gyro at +-2000dps and accel +-2G
//     */
//    hal.dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
//                       DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
//                       DMP_FEATURE_GYRO_CAL;
//    dmp_enable_feature(hal.dmp_features);
//    dmp_set_fifo_rate(DEFAULT_MPU_HZ);
//    mpu_set_dmp_state(1); // 使能DMP
//    hal.dmp_on = 1;       // 标记使能DMP
//    run_self_test();      // 自检
//    while (1)
//    {
//        unsigned long sensor_timestamp;
//        int new_data = 0;
//        get_tick(&timestamp);
//        /* Temperature data doesn't need to be read with every gyro sample.
//         * Let's make them timer-based like the compass reads.
//         */
//        /* 温度数据不需要与陀螺仪数据那每次都采样，这里设置隔一段时间采样 */
//        if (timestamp > hal.next_temp_ms)
//        {
//            hal.next_temp_ms = timestamp + TEMP_READ_MS;
//            new_temp = 1;
//        }
//        if (hal.dmp_on) // DMP打开
//        {
//            short gyro[3], accel_short[3], sensors;
//            unsigned char more;
//            long accel[3], quat[4], temperature;
//            /* This function gets new data from the FIFO when the DMP is in
//             * use. The FIFO can contain any combination of gyro, accel,
//             * quaternion, and gesture data. The sensors parameter tells the
//             * caller which data fields were actually populated with new data.
//             * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
//             * the FIFO isn't being filled with accel data.
//             * The driver parses the gesture data to determine if a gesture
//             * event has occurred; on an event, the application will be notified
//             * via a callback (assuming that a callback function was properly
//             * registered). The more parameter is non-zero if there are
//             * leftover packets in the FIFO.
//             */
//            /* 当使用DMP 时，本函数从FIFO 读取新的数据
//             * FIFO 中存储了陀螺仪、加速度、四元数及手势数据.
//             * 传感器参数可告知调用者哪种有新数据
//             * 例如, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), 那么FIFO 中就不包含加速度数据
//             * 手势数据的解算由是否产生了手势运动事件来触发
//             * 若产生了事件，应用函数会使用回调函数来通知
//             * 如果fifo有剩余的数据，more不为空
//             */
//            dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);

//            if (sensors & INV_XYZ_GYRO) // 陀螺仪数据
//            {
//                /* Push the new data to the MPL. */
//                /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
//                inv_build_gyro(gyro, sensor_timestamp);
//                new_data = 1;
//                if (new_temp) // 获取温度时间到
//                {
//                    new_temp = 0;
//                    /* Temperature only used for gyro temp comp. */
//                    /* 温度数据只用于陀螺仪的暂时计算 */
//                    mpu_get_temperature(&temperature, &sensor_timestamp);
//                    inv_build_temp(temperature, sensor_timestamp);
//                    // temperature = temperature * 1.0 / (1 << 16); // Q16格式转正常10进制
//                    // printf("temperature=%d\n", temperature);
//                }
//            }
//            if (sensors & INV_XYZ_ACCEL) // 加速度数据
//            {
//                accel[0] = (long)accel_short[0];
//                accel[1] = (long)accel_short[1];
//                accel[2] = (long)accel_short[2];
//                /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
//                inv_build_accel(accel, 0, sensor_timestamp);
//                new_data = 1;
//            }
//            if (sensors & INV_WXYZ_QUAT) // 四元数数据
//            {
//                /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
//                inv_build_quat(quat, 0, sensor_timestamp);
//                new_data = 1;
//            }
//        }
//        if (new_data)
//        {
//            /* 把新数据送入 MPL.方便调用 inv_execute_on_data()时使用 */
//            inv_execute_on_data(); // 有新数据，更新内部所有数据及状态

//            /* This function reads bias-compensated sensor data and sensor
//             * fusion outputs from the MPL. The outputs are formatted as seen
//             * in eMPL_outputs.c. This function only needs to be called at the
//             * rate requested by the host.
//             */
//            /* 本函数读取补偿的后的传感器数据和经过MPL 传感器融合后输出的数据
//                输出的格式见 in eMPL_outputs.c 文件.
//                这个函数在主机需要数据的时候调用即可，对调用频率无要求。
//            */
//            read_from_mpl();
//        }
//    }
//}
/*********************************************END OF FILE**********************/
